from naoqi import ALProxy
import locateGoal_IR as vision
import Image

motion = ALProxy("ALMotion", "localhost", 9559)
cam = ALProxy("ALVideoDevice", "localhost", 9559)
cam.setParam(18, 1)
cam.startFrameGrabber()
cam.unsubscribe("python_GVM")

def subscribe():
	cam.unsubscribe("python_GVM")
	return cam.subscribe("python_GVM", 2, 11, 5)

def snapShot(nameid):
	head = motion.getAngles(["HeadYaw", "HeadPitch"], True)
	# Make image
	shot = cam.getImageRemote(nameid)
	# Get image
	size = (640, 480)
	picture = Image.frombuffer("RGB", size, shot[6], "raw", "BGR", 0, 1)
	return (picture, head)

def findGoal(image, color, head):
	return vision.run(image, color, head)
